import lejos.navigation.TachoNavigator;
import lejos.nxt.Button;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.subsumption.Arbitrator;
import lejos.subsumption.Behavior;
import lejos.util.Timer;
import lejos.util.TimerListener;

/**
 *
 * @author Chris
 *
 */
public class DetectMarker implements TimerListener {

	private TachoNavigator tn;
	private CalibratedLS cls;
	private Timer timer;

	private int counter = 0;
	private boolean isFinished = false;
	private static final int LINE_DETECTION_TIMEOUT = 2000;

	public DetectMarker(TachoNavigator tn, CalibratedLS cls) {
		this.tn = tn;
		this.cls = cls;
		this.timer = new Timer(LINE_DETECTION_TIMEOUT, this);

		Behavior[] bArray = {foundWhiteLine, driveForward};
		Arbitrator arby = new Arbitrator(bArray, false);
		arby.start();
	}

	public static void main(String[] args) {
		SensorPort LightSensorPort = SensorPort.S1;
		Motor LeftMotor = Motor.B;
		Motor RightMotor = Motor.A;

		CalibratedLS cls = new CalibratedLS(LightSensorPort, true);
		cls.readFile();

		TachoNavigator tn = new TachoNavigator(8.16f, 14.15f, LeftMotor, RightMotor);
		tn.setSpeed(200);

		new DetectMarker(tn, cls);

		Button.waitForPress();
	}

	public void timedOut() {
		System.out.println("TIMED OUT");
		isFinished = true;
		timer.stop();
		tn.stop();
	}

	Behavior foundWhiteLine = new Behavior () {

		private boolean suppress = false;

		public void action() {
			System.out.println("FOUND_LINE");
			System.out.println(new Integer(counter).toString());
			if(counter > 0) {
				timer.stop();
			}
			counter++;
			while (cls.readValue() > 70) {
				tn.forward();
				while (!suppress) Thread.yield();
			}
			tn.stop();
			suppress = false;
		}

		public void suppress() {
			timer.start();
			isFinished = false;
			suppress = true;
			while (suppress) Thread.yield();
		}

		public boolean takeControl() {
			return cls.readValue() > 70 && !isFinished;
		}

	};

	Behavior driveForward = new Behavior () {

		public void action() {
			tn.forward();
		}

		public void suppress() {
			tn.stop();
		}

		public boolean takeControl() {
			return cls.readValue() <= 70;
		}

	};

}
